
/**
  ******************************************************************************
  * Copyright 2021 The Microbee Authors. All Rights Reserved.
  * 
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  * 
  * http://www.apache.org/licenses/LICENSE-2.0
  * 
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  * 
  * @file       gcs.c
  * @author     baiyang
  * @date       2022-11-4
  ******************************************************************************
  */

/*----------------------------------include-----------------------------------*/
#include "gcs.h"

#include <stdarg.h>
#include <string.h>
#include <stdio.h>

#include <rtthread.h>

#include <parameter/param.h>

#if CONFIG_HAL_BOARD != HAL_BOARD_SITL_WIN
#include <sensor_imu/sensor_imu.h>
#include <sensor_gps/sensor_gps.h>
#include <sensor_baro/sensor_baro.h>
#include <sensor_compass/sensor_compass.h>
#endif

#include <logger/ap_log.h>
#include <notify/notify.h>
#include <mb_arming/mb_arming.h>
#include <board_config/board_config.h>
#include <internal_error/internal_error.h>
#include <battery_monitor/battery_monitor.h>
/*-----------------------------------macro------------------------------------*/
#ifndef MIN
#define MIN(a, b) (a < b ? a : b)
#endif

#ifndef MAX
#define MAX(a, b) (a > b ? a : b)
#endif

#if CONFIG_HAL_BOARD != HAL_BOARD_SITL_WIN
_Static_assert(NUM_STREAMS == GCS_MAVLINK_NUM_STREAM_RATES, "num streams must equal num stream rates");
#else
static_assert(NUM_STREAMS == GCS_MAVLINK_NUM_STREAM_RATES, "num streams must equal num stream rates");
#endif
/*----------------------------------typedef-----------------------------------*/

/*---------------------------------prototype----------------------------------*/
static void send_textv2(gcs_t gcs_singleton, MAV_SEVERITY severity, const char *fmt, va_list arg_list, uint8_t dest_bitmask);
/*----------------------------------variable----------------------------------*/
static struct gcs gcs_obj;
/*-------------------------------------os-------------------------------------*/

/*----------------------------------function----------------------------------*/
gcs_t gcs_get_singleton()
{
    return &gcs_obj;
}

void gcs_ctor(gcs_ops_t front_ops)
{
    RT_ASSERT(front_ops != RT_NULL);

    gcs_obj.ops = front_ops;

    status_text_queue_ctor(&gcs_obj._statustext_queue);
}

void gcs_init()
{
    mavlink_system.sysid = gcs_sysid_this_mav();
}

gcs_mavlink_t gcs_chan(const uint8_t ofs)
{
    if (ofs >= gcs_obj._num_gcs) {
        INTERNAL_ERROR(internal_error_gcs_offset);
        return NULL;
    }
    return gcs_obj._chan[ofs];
}

const gcs_mavlink_t gcs_chan_const(const uint8_t ofs)
{
    if (ofs >= gcs_obj._num_gcs) {
        INTERNAL_ERROR(internal_error_gcs_offset);
        return NULL;
    }
    return gcs_obj._chan[ofs];
}

void gcs_create_gcs_mavlink_backend(gcs_mavlink_parameters_t params, rt_device_t uart)
{
    if (gcs_obj._num_gcs >= ARRAY_SIZE(gcs_obj.chan_parameters)) {
        return;
    }

    gcs_obj._chan[gcs_obj._num_gcs] = gcs_new_gcs_mavlink_backend(params, uart);
    if (gcs_obj._chan[gcs_obj._num_gcs] == NULL) {
        return;
    }

    if (!gcsmav_init(gcs_obj._chan[gcs_obj._num_gcs], gcs_obj._num_gcs)) {
        rt_free(gcs_obj._chan[gcs_obj._num_gcs]);
        gcs_obj._chan[gcs_obj._num_gcs] = NULL;
        return;
    }

    gcs_obj._num_gcs++;
}

void gcs_setup_console()
{
    rt_device_t uart = SerialManager_find_protocol_instance(SerialProtocol_MAVLink, 0);
    if (uart == NULL) {
        // this is probably not going to end well.
        return;
    }
    if (ARRAY_SIZE(gcs_obj.chan_parameters) == 0) {
        return;
    }
    gcs_create_gcs_mavlink_backend(&gcs_obj.chan_parameters[0], uart);
}

void gcs_setup_uarts()
{
    for (uint8_t i = 1; i < MAVLINK_COMM_NUM_BUFFERS; i++) {
        if (i >= ARRAY_SIZE(gcs_obj.chan_parameters)) {
            // should not happen
            break;
        }
        rt_device_t uart = SerialManager_find_protocol_instance(SerialProtocol_MAVLink, i);
        if (uart == NULL) {
            // no more mavlink uarts
            break;
        }
        gcs_create_gcs_mavlink_backend(&gcs_obj.chan_parameters[i], uart);
    }

#if 0
    if (frsky == nullptr) {
        frsky = new AP_Frsky_Telem();
        if (frsky == nullptr || !frsky->init()) {
            delete frsky;
            frsky = nullptr;
        }
    }
#if AP_LTM_TELEM_ENABLED
    ltm_telemetry.init();
#endif

#if AP_DEVO_TELEM_ENABLED
    devo_telemetry.init();
#endif
#endif
}

uint8_t gcs_num_gcs()
{
    return gcs_obj._num_gcs;
}

void gcs_out_of_space_to_send(mavlink_channel_t chan)
{
    if (gcs_obj._chan[(uint8_t)chan] != NULL) {
        gcsmav_out_of_space_to_send(gcs_obj._chan[(uint8_t)chan]);
    }
}

/*
 * returns a mask of channels that statustexts should be sent to
 */
uint8_t gcs_statustext_send_channel_mask()
{
    uint8_t ret = 0;
    ret |= gcsmav_active_channel_mask();
    ret |= gcsmav_streaming_channel_mask();
    ret &= ~(gcsmav_private_channel_mask());
    return ret;
}

void gcs_send_text(MAV_SEVERITY severity, const char *fmt, ...)
{
    va_list arg_list;
    va_start(arg_list, fmt);
    gcs_send_textv(severity, fmt, arg_list);
    va_end(arg_list);
}

void gcs_send_text2(uint8_t chan, MAV_SEVERITY severity, const char *fmt, ...)
{
    va_list arg_list;
    va_start(arg_list, fmt);
    gcs_send_textv2(severity, fmt, arg_list, (1<<chan));
    va_end(arg_list);
}

/*
  send a text message to all GCS
 */
void gcs_send_textv(MAV_SEVERITY severity, const char *fmt, va_list arg_list)
{
    uint8_t mask = gcs_statustext_send_channel_mask();
    if (!gcs_obj.update_send_has_been_called) {
        // we have not yet initialised the streaming-channel-mask,
        // which is done as part of the update() call.  So just send
        // it to all channels:
        mask = (1<<gcs_obj._num_gcs)-1;
    }
    gcs_send_textv2(severity, fmt, arg_list, mask);
}

/*
    send a statustext text string to specific MAVLink bitmask
*/
void gcs_send_textv2(MAV_SEVERITY severity, const char *fmt, va_list arg_list, uint8_t dest_bitmask)
{
    if (gcs_obj.ops != NULL && gcs_obj.ops->send_textv != NULL) {
        gcs_obj.ops->send_textv(&gcs_obj, severity, fmt, arg_list, dest_bitmask);
        return;
    }

    send_textv2(&gcs_obj, severity, fmt, arg_list, dest_bitmask);
}

void gcs_service_statustext(void)
{
    status_text_queue_take_blocking(&gcs_obj._statustext_queue);

    if (status_text_queue_is_empty(&gcs_obj._statustext_queue)) {
        // nothing to do
        status_text_queue_give(&gcs_obj._statustext_queue);
        return;
    }

    for (uint8_t i=gcs_obj.first_backend_to_send; i<gcs_obj._num_gcs; i++) {
        gcsmav_service_statustext(gcs_obj._chan[i]);
    }

    for (uint8_t i=0; i<gcs_obj.first_backend_to_send; i++) {
        gcsmav_service_statustext(gcs_obj._chan[i]);
    }

    status_text_queue_prune(&gcs_obj._statustext_queue);

    status_text_queue_give(&gcs_obj._statustext_queue);
}

status_text_queue_t gcs_statustext_queue()
{
    return &gcs_obj._statustext_queue;
}

/*
  install an alternative protocol handler. This allows another
  protocol to take over the link if MAVLink goes idle. It is used to
  allow for the AP_BLHeli pass-thru protocols to run on hal.serial(0)
 */
bool gcs_install_alternative_protocol(mavlink_channel_t c, protocol_handler_fn_t handler)
{
    if (c >= gcs_obj._num_gcs) {
        return false;
    }
    if (gcs_chan(c)->alternative.handler && handler) {
        // already have one installed - we may need to add support for
        // multiple alternative handlers
        return false;
    }
    gcs_chan(c)->alternative.handler = handler;
    return true;
}

void gcs_get_sensor_status_flags(uint32_t *present, uint32_t *enabled, uint32_t *health)
{
    gcs_update_sensor_status_flags();

    *present = gcs_obj.control_sensors_present;
    *enabled = gcs_obj.control_sensors_enabled;
    *health = gcs_obj.control_sensors_health;
}

void gcs_update_sensor_status_flags()
{
    gcs_obj.control_sensors_present = 0;
    gcs_obj.control_sensors_enabled = 0;
    gcs_obj.control_sensors_health = 0;

#if CONFIG_HAL_BOARD != HAL_BOARD_SITL_WIN
    gcs_obj.control_sensors_present |= MAV_SYS_STATUS_AHRS;
    gcs_obj.control_sensors_enabled |= MAV_SYS_STATUS_AHRS;
    gcs_obj.control_sensors_health |= MAV_SYS_STATUS_AHRS;

    if (sensor_compass_available()) {
      gcs_obj.control_sensors_present |= MAV_SYS_STATUS_SENSOR_3D_MAG;
      gcs_obj.control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_3D_MAG;
    }
    if (sensor_compass_available() && sensor_compass_healthy()) {
      gcs_obj.control_sensors_health |= MAV_SYS_STATUS_SENSOR_3D_MAG;
    }

    gcs_obj.control_sensors_present |= MAV_SYS_STATUS_SENSOR_ABSOLUTE_PRESSURE;
    gcs_obj.control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_ABSOLUTE_PRESSURE;
    if (sensor_baro_all_healthy()) {
      gcs_obj.control_sensors_health |= MAV_SYS_STATUS_SENSOR_ABSOLUTE_PRESSURE;
    }

    if (sensor_gps_status() > NO_GPS) {
      gcs_obj.control_sensors_present |= MAV_SYS_STATUS_SENSOR_GPS;
      gcs_obj.control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_GPS;
    }
    if (sensor_gps_is_healthy() && sensor_gps_status() >= NO_FIX) {
      gcs_obj.control_sensors_health |= MAV_SYS_STATUS_SENSOR_GPS;
    }

    gcs_obj.control_sensors_present |= MAV_SYS_STATUS_SENSOR_3D_GYRO;
    gcs_obj.control_sensors_present |= MAV_SYS_STATUS_SENSOR_3D_ACCEL;
    if (!sensor_imu_calibrating()) {
        gcs_obj.control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_3D_ACCEL;
        gcs_obj.control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_3D_GYRO;
        if (sensor_imu_get_accel_health_all()) {
            gcs_obj.control_sensors_health |= MAV_SYS_STATUS_SENSOR_3D_ACCEL;
        }
        if (sensor_imu_get_gyro_health_all() && sensor_imu_gyro_calibrated_ok_all()) {
            gcs_obj.control_sensors_health |= MAV_SYS_STATUS_SENSOR_3D_GYRO;
        }
    }

    battery_monitor_t battery = battmonitor_get_singleton();
    gcs_obj.control_sensors_present |= MAV_SYS_STATUS_SENSOR_BATTERY;
    if (battery->_num_instances > 0) {
        gcs_obj.control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_BATTERY;
    }
    if (battmonitor_healthy() && !battmonitor_has_failsafed()) {
        gcs_obj.control_sensors_health |= MAV_SYS_STATUS_SENSOR_BATTERY;
    }
#endif

    gcs_obj.control_sensors_present |= MAV_SYS_STATUS_LOGGING;
    gcs_obj.control_sensors_enabled |= MAV_SYS_STATUS_LOGGING;
    gcs_obj.control_sensors_health |= MAV_SYS_STATUS_LOGGING;

    // set motors outputs as enabled if safety switch is not disarmed (i.e. either NONE or ARMED)
    gcs_obj.control_sensors_present |= MAV_SYS_STATUS_SENSOR_MOTOR_OUTPUTS;
    if (brd_safety_switch_state() != SAFETY_DISARMED) {
      gcs_obj.control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_MOTOR_OUTPUTS;
    }
    gcs_obj.control_sensors_health |= MAV_SYS_STATUS_SENSOR_MOTOR_OUTPUTS;

#if CONFIG_HAL_BOARD == HAL_BOARD_SITL_WIN
    // always show EKF type 10 as healthy. This prevents spurious error
    // messages in xplane and other simulators that use EKF type 10
    gcs_obj.control_sensors_present |= MAV_SYS_STATUS_AHRS | MAV_SYS_STATUS_SENSOR_GPS | MAV_SYS_STATUS_SENSOR_3D_ACCEL | MAV_SYS_STATUS_SENSOR_3D_GYRO;
    gcs_obj.control_sensors_enabled |= MAV_SYS_STATUS_AHRS | MAV_SYS_STATUS_SENSOR_GPS | MAV_SYS_STATUS_SENSOR_3D_ACCEL | MAV_SYS_STATUS_SENSOR_3D_GYRO;
    gcs_obj.control_sensors_health |= MAV_SYS_STATUS_AHRS | MAV_SYS_STATUS_SENSOR_GPS | MAV_SYS_STATUS_SENSOR_3D_ACCEL | MAV_SYS_STATUS_SENSOR_3D_GYRO;
#endif

    mb_arming_t arming = mb_arming_get_singleton();

    // give GCS status of prearm checks. This is enabled if any arming checks are enabled.
    // it is healthy if armed or checks are passing
    gcs_obj.control_sensors_present |= MAV_SYS_STATUS_PREARM_CHECK;
    if (arming != NULL && arming->checks_to_perform) {
        gcs_obj.control_sensors_enabled |= MAV_SYS_STATUS_PREARM_CHECK;
        if (brd_safety_switch_state() || notify_flags.pre_arm_check) {
            gcs_obj.control_sensors_health |= MAV_SYS_STATUS_PREARM_CHECK;
        }
    }

    gcs_update_vehicle_sensor_status_flags();
}

void gcs_send_to_active_channels(uint32_t msgid, const char *pkt)
{
    const mavlink_msg_entry_t *entry = mavlink_get_msg_entry(msgid);
    if (entry == NULL) {
        return;
    }
    for (uint8_t i=0; i<gcs_obj._num_gcs; i++) {
        gcs_mavlink_t c = gcs_chan(i);
        if (gcsmav_is_private(c->chan)) {
            continue;
        }
        if (!gcsmav_is_active(c)) {
            continue;
        }
        if (entry->max_msg_len + gcsmav_packet_overhead(c) > gcsmav_txspace(c)) {
            // no room on this channel
            continue;
        }
        gcsmav_send_message3(c, pkt, entry);
    }
}

void gcs_send_named_float(const char *name, float value)
{

    mavlink_named_value_float_t packet = {0};
    packet.time_boot_ms = time_millis();
    packet.value = value;
    rt_memcpy(packet.name, name, MIN(rt_strlen(name), (uint8_t)MAVLINK_MSG_NAMED_VALUE_FLOAT_FIELD_NAME_LEN));

    gcs_send_to_active_channels(MAVLINK_MSG_ID_NAMED_VALUE_FLOAT,
                                  (const char *)&packet);
}

#if HAL_HIGH_LATENCY2_ENABLED
void gcs_enable_high_latency_connections(bool enabled)
{
    for (uint8_t i=0; i<gcs_obj._num_gcs; i++) {
        gcs_mavlink_t c = gcs_chan(i);
        c->high_latency_link_enabled = enabled && c->is_high_latency_link;
    } 
    gcs_send_text(MAV_SEVERITY_NOTICE, "High Latency %s", enabled ? "enabled" : "disabled");
}
#endif // HAL_HIGH_LATENCY2_ENABLED

void gcs_send_message(enum mb_message id)
{
    for (uint8_t i=0; i<gcs_obj._num_gcs; i++) {
        gcsmav_send_message(gcs_chan(i), id);
    }
}

void gcs_send_message2(uint8_t chan, enum mb_message id)
{
    if (chan >= gcs_obj._num_gcs) {
        return;
    }

    gcsmav_send_message(gcs_chan(chan), id);
}

void gcs_update_send()
{
    gcs_obj.update_send_has_been_called = true;

#if 0
    if (!gcs_obj.initialised_missionitemprotocol_objects) {
        gcs_obj.initialised_missionitemprotocol_objects = true;
        // once-only initialisation of MissionItemProtocol objects:
#if AP_MISSION_ENABLED
        AP_Mission *mission = AP::mission();
        if (mission != nullptr) {
            missionitemprotocols[MAV_MISSION_TYPE_MISSION] = new MissionItemProtocol_Waypoints(*mission);
        }
#endif
#if HAL_RALLY_ENABLED
        AP_Rally *rally = AP::rally();
        if (rally != nullptr) {
            missionitemprotocols[MAV_MISSION_TYPE_RALLY] = new MissionItemProtocol_Rally(*rally);
        }
#endif
#if AP_FENCE_ENABLED
        AC_Fence *fence = AP::fence();
        if (fence != nullptr) {
            missionitemprotocols[MAV_MISSION_TYPE_FENCE] = new MissionItemProtocol_Fence(*fence);
        }
#endif
    }

    for (auto *prot : missionitemprotocols) {
        if (prot == nullptr) {
            continue;
        }
        prot->update();
    }
#endif

    // round-robin the GCS_MAVLINK backend that gets to go first so
    // one backend doesn't monopolise all of the time allowed for sending
    // messages
    for (uint8_t i=gcs_obj.first_backend_to_send; i<gcs_obj._num_gcs; i++) {
        gcsmav_update_send(gcs_chan(i));
    }
    for (uint8_t i=0; i<gcs_obj.first_backend_to_send; i++) {
        gcsmav_update_send(gcs_chan(i));
    }

    gcs_service_statustext();

    gcs_obj.first_backend_to_send++;
    if (gcs_obj.first_backend_to_send >= gcs_obj._num_gcs) {
        gcs_obj.first_backend_to_send = 0;
    }
}

void gcs_update_receive(void)
{
    for (uint8_t i=0; i<gcs_obj._num_gcs; i++) {
        gcsmav_update_receive(gcs_chan(i), 1000);
    }

    // also update UART pass-thru, if enabled
    //gcs_update_passthru();
}

void gcs_send_mission_item_reached_message(uint16_t mission_index)
{
    for (uint8_t i=0; i<gcs_obj._num_gcs; i++) {
        gcs_chan(i)->mission_item_reached_index = mission_index;
        gcsmav_send_message(gcs_chan(i), MAV_MSG_MISSION_ITEM_REACHED);
    }
}

/*
  this function is reserved for use by scripting
 */
MAV_RESULT gcs_set_message_interval(uint8_t port_num, uint32_t msg_id, int32_t interval_us)
{
    uint8_t channel = gcs_get_channel_from_port_number(port_num);

    if ((channel < MAVLINK_COMM_NUM_BUFFERS) && (gcs_chan(channel) != NULL)) {
        return gcsmav_set_message_interval(gcs_chan(channel), msg_id, interval_us);
    }

    return MAV_RESULT_FAILED;
}

uint8_t gcs_get_channel_from_port_number(uint8_t port_num)
{
    rt_device_t u = SerialManager_get_serial_by_id(port_num);
    for (uint8_t i=0; i<gcs_obj._num_gcs; i++) {
        if (gcsmav_get_uart(gcs_chan(i)) == u) {
            return i;
        }
    }

    return UINT8_MAX;
}

// last time traffic was seen from my designated GCS.  traffic
// includes heartbeats and some manual control messages.
uint32_t gcs_sysid_myggcs_last_seen_time_ms()
{
    return gcs_obj._sysid_mygcs_last_seen_time_ms;
}

// called when valid traffic has been seen from our GCS
void gcs_sysid_myggcs_seen(uint32_t seen_time_ms)
{
    gcs_obj._sysid_mygcs_last_seen_time_ms = seen_time_ms;
}

/*
  send a parameter value message to all active MAVLink connections
 */
void gcs_send_parameter_value(const char *param_name, enum param_type_t param_type, float param_value)
{
    mavlink_param_value_t packet = {0};
    const uint8_t to_copy = MIN(ARRAY_SIZE(packet.param_id), rt_strlen(param_name));
    rt_memcpy(packet.param_id, param_name, to_copy);
    packet.param_value = param_value;
    packet.param_type = gcsmav_mav_param_type(param_type);
    packet.param_count = param_get_count();
    packet.param_index = -1;

    gcs_send_to_active_channels(MAVLINK_MSG_ID_PARAM_VALUE, (const char *)&packet);

    aplog_write_parameter(param_name, param_value, aplog_quiet_nanf());
}


/// ops
uint16_t gcs_min_loop_time_remaining_for_message_send_us()
{
    if (gcs_obj.ops != NULL && gcs_obj.ops->min_loop_time_remaining_for_message_send_us != NULL) {
        return gcs_obj.ops->min_loop_time_remaining_for_message_send_us(&gcs_obj);
    }

    return 200;
}

bool gcs_vehicle_initialised()
{
    if (gcs_obj.ops != NULL && gcs_obj.ops->vehicle_initialised != NULL) {
        return gcs_obj.ops->vehicle_initialised(&gcs_obj);
    }

    return true;
}

bool gcs_simple_input_active()
{
    if (gcs_obj.ops != NULL && gcs_obj.ops->simple_input_active != NULL) {
        return gcs_obj.ops->simple_input_active(&gcs_obj);
    }

    return false;
}

bool gcs_supersimple_input_active()
{
    if (gcs_obj.ops != NULL && gcs_obj.ops->supersimple_input_active != NULL) {
        return gcs_obj.ops->supersimple_input_active(&gcs_obj);
    }

    return false;
}

uint8_t gcs_sysid_this_mav()
{
    RT_ASSERT(gcs_obj.ops != RT_NULL);
    RT_ASSERT(gcs_obj.ops->sysid_this_mav != RT_NULL);

    return gcs_obj.ops->sysid_this_mav(&gcs_obj);
}

uint8_t gcs_sysid_my_gcs()
{
    RT_ASSERT(gcs_obj.ops != RT_NULL);
    RT_ASSERT(gcs_obj.ops->sysid_my_gcs != RT_NULL);

    return gcs_obj.ops->sysid_my_gcs(&gcs_obj);
}

gcs_mavlink_t gcs_new_gcs_mavlink_backend(gcs_mavlink_parameters_t params, rt_device_t uart)
{
    RT_ASSERT(gcs_obj.ops != RT_NULL);
    RT_ASSERT(gcs_obj.ops->new_gcs_mavlink_backend != RT_NULL);

    return gcs_obj.ops->new_gcs_mavlink_backend(&gcs_obj, params, uart);
}

void gcs_update_vehicle_sensor_status_flags()
{
    if (gcs_obj.ops != NULL && gcs_obj.ops->update_vehicle_sensor_status_flags != NULL) {
        gcs_obj.ops->update_vehicle_sensor_status_flags(&gcs_obj);
    }
}

enum GPS_Status gcs_min_status_for_gps_healthy()
{
    if (gcs_obj.ops != NULL && gcs_obj.ops->min_status_for_gps_healthy != NULL) {
        return gcs_obj.ops->min_status_for_gps_healthy(&gcs_obj);
    }

    return NO_FIX;
}

/*
    send a statustext text string to specific MAVLink bitmask
*/
static void send_textv2(gcs_t gcs_singleton, MAV_SEVERITY severity, const char *fmt, va_list arg_list, uint8_t dest_bitmask)
{
    char first_piece_of_text[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN+1] = {0};

    status_text_queue_take_blocking(&gcs_singleton->_statustext_queue);

    do {
        // send_text can be called from multiple threads; we must
        // protect the "text" member with _statustext_sem

        vsnprintf(gcs_singleton->statustext_printf_buffer, sizeof(gcs_singleton->statustext_printf_buffer), fmt, arg_list);
        rt_memcpy(first_piece_of_text, gcs_singleton->statustext_printf_buffer, ARRAY_SIZE(first_piece_of_text)-1);

        // filter destination ports to only allow active ports.
        struct statustext_t statustext = {0};
        if (gcs_singleton->update_send_has_been_called) {
            statustext.bitmask = gcs_statustext_send_channel_mask();
        } else {
            // we have not yet initialised the streaming-channel-mask,
            // which is done as part of the update() call.  So just send
            // it to all channels:
            statustext.bitmask = (1<<gcs_singleton->_num_gcs)-1;
        }
        statustext.bitmask &= dest_bitmask;
        if (!statustext.bitmask) {
            // nowhere to send
            break;
        }

        statustext.entry_created_ms = time_millis16();
        statustext.msg.severity = severity;

        static uint16_t msgid;
        if (rt_strlen(gcs_singleton->statustext_printf_buffer) > sizeof(statustext.msg.text)) {
            msgid++;
            if (msgid == 0) {
                msgid = 1;
            }
            statustext.msg.id = msgid;
        }

        const char *remainder = gcs_singleton->statustext_printf_buffer;
        for (uint8_t i=0; i<GCS_STATUS_CAPACITY; i++) {
            statustext.msg.chunk_seq = i;
            const size_t remainder_len = rt_strlen(remainder);
            // note that remainder_len may be zero here!
            uint16_t n = MIN(sizeof(statustext.msg.text), remainder_len);
            if (i == GCS_STATUS_CAPACITY -1 && n == sizeof(statustext.msg.text)) {
                // fantastic.  This us a very long statustext and
                // we've actually managed to push everything else out
                // of the queue - this is the last chunk, so we MUST
                // null-terminate.
                n -= 1;
            }
            rt_memset(statustext.msg.text, '\0', sizeof(statustext.msg.text));
            rt_memcpy(statustext.msg.text, remainder, n);
            // The force push will ensure comm links do not block other comm links forever if they fail.
            // If we push to a full buffer then we overwrite the oldest entry, effectively removing the
            // block but not until the buffer fills up.
            status_text_queue_push_force(&gcs_singleton->_statustext_queue, &statustext);
            remainder = &remainder[n];

            // note that remainder_len here is the remainder length for
            // the *old* remainder!
            if (remainder_len < sizeof(statustext.msg.text) || statustext.msg.id == 0) {
                break;
            }
        }
    } while (false);

    status_text_queue_give(&gcs_singleton->_statustext_queue);

    // given we don't really know what these methods get up to, we
    // don't hold the statustext semaphore while doing them:
    aplog_write_message(first_piece_of_text);

#if 0
    frsky = AP::frsky_telem();
    if (frsky != nullptr) {
        frsky->queue_message(severity, first_piece_of_text);
    }
#if HAL_SPEKTRUM_TELEM_ENABLED
    AP_Spektrum_Telem* spektrum = AP::spektrum_telem();
    if (spektrum != nullptr) {
        spektrum->queue_message(severity, first_piece_of_text);
    }
#endif
#if HAL_CRSF_TELEM_ENABLED
    AP_CRSF_Telem* crsf = AP::crsf_telem();
    if (crsf != nullptr) {
        crsf->queue_message(severity, first_piece_of_text);
    }
#endif
    AP_Notify *notify = AP_Notify::get_singleton();
    if (notify) {
        notify->send_text(first_piece_of_text);
    }
#endif

    // push the messages out straight away until the vehicle states
    // that it is initialised.  At that point we can assume
    // update_send is being called
    if (!gcs_vehicle_initialised()) {
        gcs_service_statustext();
    }
}
/*------------------------------------test------------------------------------*/


